#include "vehicle.h"
#include "math/segment2d.h"
#include "utils.h"

#include <iomanip>

using namespace decision::adas;

Vehicle::Vehicle(int64_t id)
    : logger_(rclcpp::get_logger("Vehicle[" + std::to_string(id) + "]"))
    , trackid_(id)
{
    decision::math::Vec2d vec2d(0, 0);
    box2d_ = std::make_unique<decision::math::Box2d>(vec2d, M_PI_2, 5, 2);
}

int64_t Vehicle::trackid() const
{
    return trackid_;
}

double Vehicle::x() const
{
    return box2d_->center().x();
}

double Vehicle::y() const
{
    return box2d_->center().y();
}

double Vehicle::heading() const
{
    return box2d_->heading();
}

double Vehicle::length() const
{
    return box2d_->length();
}

double Vehicle::width() const
{
    return box2d_->width();
}

std::string Vehicle::debug_string() const
{
    std::stringstream ss;
    ss << std::fixed << std::setprecision(2)
       << "tid:" << trackid_
       << " x/y:" << box2d_->center().x() << "," << box2d_->center().y()
       << " hea:" << box2d_->heading() / M_PI * 180 << "/" << legalize_angle((M_PI_2 - box2d_->heading()) / M_PI * 180)
       << " l/w:" << box2d_->length() << "," << box2d_->width();
    return ss.str();
}

double Vehicle::distance(const Vehicle& other) const
{
    return box2d_->distance_to(*(other.box2d_));
}

double Vehicle::azimuth(const Vehicle& other) const
{
    decision::math::Segment2d vec(box2d_->center(), other.box2d_->center());
    return vec.heading();
}

void Vehicle::update(double x, double y, double hea, double length, double width)
{
    std::lock_guard<std::mutex> guard(lock_);

    decision::math::Vec2d vec2d(x, y);
    box2d_ = std::make_unique<decision::math::Box2d>(vec2d, hea, length, width);

    update_time_ = muduo::Timestamp::now();
}

muduo::Timestamp Vehicle::get_update_time()
{
    return update_time_;
}
